#include "precompiled.h"
#pragma hdrstop

namespace inf {

    const Vec2 Vec2::ORIGIN( 0.0f, 0.0f );
    const Vec2 Vec2::ONE( 1.0f, 1.0f );
    const Vec2& Vec2::ZERO( Vec2::ORIGIN );

    const Vec3 Vec3::ORIGIN( 0.0f, 0.0f, 0.0f );
    const Vec3 Vec3::ONE( 1.0f, 1.0f, 1.0f );
    const Vec3& Vec3::ZERO = Vec3::ORIGIN;

    const Vec4 Vec4::ORIGIN( 0.0f, 0.0f, 0.0f, 0.0f );
    const Vec4 Vec4::ONE( 1.0f, 1.0f, 1.0f, 1.0f );
    const Vec4 Vec4::ZERO( Vec4::ORIGIN );


    //===============================================================
    //
    //	Vec2
    //
    //===============================================================

    /*
    =============
    Vec2::to_string
    =============
    */
    const char *Vec2::to_string( int precision ) const {
        return float_array_to_string_ANSI( to_float_ptr(), get_dimension(), precision );
    }

    /*
    =============
    lerp

    Linearly inperpolates one vector to another.
    =============
    */
    void Vec2::lerp( const Vec2 &v1, const Vec2 &v2, const float l ) {
        if ( l <= 0.0f ) {
            (*this) = v1;
        } else if ( l >= 1.0f ) {
            (*this) = v2;
        } else {
            (*this) = v1 + l * ( v2 - v1 );
        }
    }


    //===============================================================
    //
    //	Vec3
    //
    //===============================================================

    /*
    =============
    Vec3::to_yaw
    =============
    */
    float Vec3::to_yaw( void ) const {
        float yaw;

        if ( ( y == 0.0f ) && ( x == 0.0f ) ) {
            yaw = 0.0f;
        } else {
            yaw = math::RAD2DEG( math::atan( y, x ) );
            if ( yaw < 0.0f ) {
                yaw += 360.0f;
            }
        }

        return yaw;
    }

    /*
    =============
    Vec3::to_pitch
    =============
    */
    float Vec3::to_pitch( void ) const {
        float	forward;
        float	pitch;

        if ( ( x == 0.0f ) && ( y == 0.0f ) ) {
            if ( z > 0.0f ) {
                pitch = 90.0f;
            } else {
                pitch = 270.0f;
            }
        } else {
            forward = ( float )math::sqrt( x * x + y * y );
            pitch = math::RAD2DEG( math::atan( z, forward ) );
            if ( pitch < 0.0f ) {
                pitch += 360.0f;
            }
        }

        return pitch;
    }

    /*
    =============
    Vec3::to_Angles
    =============
    */
    Angles Vec3::to_Angles( void ) const {
        float forward;
        float yaw;
        float pitch;

        if ( ( x == 0.0f ) && ( y == 0.0f ) ) {
            yaw = 0.0f;
            if ( z > 0.0f ) {
                pitch = 90.0f;
            } else {
                pitch = 270.0f;
            }
        } else {
            yaw = math::RAD2DEG( math::atan( y, x ) );
            if ( yaw < 0.0f ) {
                yaw += 360.0f;
            }

            forward = ( float )math::sqrt( x * x + y * y );
            pitch = math::RAD2DEG( math::atan( z, forward ) );
            if ( pitch < 0.0f ) {
                pitch += 360.0f;
            }
        }

        return Angles( -pitch, yaw, 0.0f );
    }

    /*
    =============
    Vec3::to_radians
    =============
    */
    Angles Vec3::to_radians( void ) const 
    {
        float forward;
        float yaw;
        float pitch;

        if( !x && !y ) 
        {
            yaw = 0.0f;
            if( z > 0.0f ) 
            {
                pitch = math::HALF_PI;
            }
            else 
            {
                pitch = math::THREEFOURTHS_PI;
            }
        } 
        else 
        {
            yaw = math::atan64( y, x );
            if( yaw < 0.0f ) 
            {
                yaw += math::TWO_PI;
            }

            forward = ( float )math::sqrt( x * x + y * y );
            pitch = math::atan64( z, forward );
            if( pitch < 0.0f ) 
            {
                pitch += math::TWO_PI;
            }
        }

        return Angles( -pitch, yaw, 0.0f );
    }

    /*
    =============
    Vec3::to_Polar
    =============
    */
    Polar3 Vec3::to_Polar( void ) const {
    	float forward;
    	float yaw;
    	float pitch;
    	
    	if ( ( x == 0.0f ) && ( y == 0.0f ) ) {
    		yaw = 0.0f;
    		if ( z > 0.0f ) {
    			pitch = 90.0f;
    		} else {
    			pitch = 270.0f;
    		}
    	} else {
    		yaw = math::RAD2DEG( math::atan( y, x ) );
    		if ( yaw < 0.0f ) {
    			yaw += 360.0f;
    		}
    
    		forward = ( float )math::sqrt( x * x + y * y );
    		pitch = math::RAD2DEG( math::atan( z, forward ) );
    		if ( pitch < 0.0f ) {
    			pitch += 360.0f;
    		}
    	}
    	return Polar3( math::sqrt( x * x + y * y + z * z ), yaw, -pitch );
    }

    /*
    =============
    Vec3::to_Mat3
    =============
    */
    Mat3 Vec3::to_Mat3( void ) const {
        Mat3	mat;
        float	d;

        mat[0] = *this;
        d = x * x + y * y;
        if ( !d ) {
            mat[1][0] = 1.0f;
            mat[1][1] = 0.0f;
            mat[1][2] = 0.0f;
        } else {
            d = math::inv_sqrt( d );
            mat[1][0] = -y * d;
            mat[1][1] = x * d;
            mat[1][2] = 0.0f;
        }
        mat[2] = cross( mat[1] );

        return mat;
    }

    // jscott: slightly quicker version without the copy
    Mat3 &Vec3::to_Mat3( Mat3 &mat ) const {
        float	d;

        mat[0] = *this;
        d = x * x + y * y;
        if ( !d ) {
            mat[1][0] = 1.0f;
            mat[1][1] = 0.0f;
            mat[1][2] = 0.0f;
        } else {
            d = math::inv_sqrt( d );
            mat[1][0] = -y * d;
            mat[1][1] = x * d;
            mat[1][2] = 0.0f;
        }
        mat[2] = cross( mat[1] );
        return( mat );
    }

    /*
    =============
    Vec3::to_string
    =============
    */
    const char *Vec3::to_string( int precision ) const {
        return float_array_to_string_ANSI( to_float_ptr(), get_dimension(), precision );
    }

    /*
    =============
    lerp

    Linearly inperpolates one vector to another.
    =============
    */
    void Vec3::lerp( const Vec3 &v1, const Vec3 &v2, const float l ) {
        if ( l <= 0.0f ) {
            (*this) = v1;
        } else if ( l >= 1.0f ) {
            (*this) = v2;
        } else {
            (*this) = v1 + l * ( v2 - v1 );
        }

        VEC_CHECK_BAD( *this );
    }

    /*
    =============
    slerp

    Spherical linear interpolation from v1 to v2.
    Vectors are expected to be normalized.
    =============
    */
#define LERP_DELTA 1e-6

    void Vec3::slerp( const Vec3 &v1, const Vec3 &v2, const float t ) {
        float omega, cosom, sinom, scale0, scale1;

        if ( t <= 0.0f ) {
            (*this) = v1;
            return;
        } else if ( t >= 1.0f ) {
            (*this) = v2;
            return;
        }

        cosom = v1 * v2;
        if ( ( 1.0f - cosom ) > LERP_DELTA ) {
            omega = math::acos( cosom );
            sinom = math::sin( omega );
            scale0 = math::sin( ( 1.0f - t ) * omega ) / sinom;
            scale1 = math::sin( t * omega ) / sinom;
        } else {
            scale0 = 1.0f - t;
            scale1 = t;
        }

        (*this) = ( v1 * scale0 + v2 * scale1 );
        VEC_CHECK_BAD( *this );
    }

    /*
    =============
    project_self_onto_sphere

    Projects the z component onto a sphere.
    =============
    */
    void Vec3::project_self_onto_sphere( const float radius ) {
        float rsqr = radius * radius;
        float len = length();
        if ( len  < rsqr * 0.5f ) {
            z = math::sqrt( rsqr - len );
        } else {
            z = rsqr / ( 2.0f * math::sqrt( len ) );
        }
        VEC_CHECK_BAD( *this );
    }



    //===============================================================
    //
    //	Vec4
    //
    //===============================================================

    /*
    =============
    Vec4::to_string
    =============
    */
    const char *Vec4::to_string( int precision ) const {
        return float_array_to_string_ANSI( to_float_ptr(), get_dimension(), precision );
    }

    /*
    =============
    lerp

    Linearly inperpolates one vector to another.
    =============
    */
    void Vec4::lerp( const Vec4 &v1, const Vec4 &v2, const float l ) {
        if ( l <= 0.0f ) {
            (*this) = v1;
        } else if ( l >= 1.0f ) {
            (*this) = v2;
        } else {
            (*this) = v1 + l * ( v2 - v1 );
        }
    }


}   // ::inf
